// opencvCalibration.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <cv.h>
#include <vector>
#include <fstream>
#include <iostream>
using namespace cv;
using namespace std;
#pragma comment(lib,"opencv_calib3d242d.lib");
#pragma comment(lib,"opencv_core242d.lib");
#define meetSortX(vv,thresholdSort) \
	abs(vv[0].x-vv[1].x)>thresholdSort&&abs(vv[1].x-vv[2].x)>thresholdSort
#define meetSortY(vv,thresholdSort) \
	abs(vv[0].y-vv[1].y)>thresholdSort&&abs(vv[1].y-vv[2].y)>thresholdSort
template<class T>
bool sortcoodx(T& p1,T& p2)
{return p1.x<p2.x;}
template<class T>
bool sortcoody(T& p1,T& p2)
{return p1.y<p2.y;}
template<class T>
bool sortcoodz(T& p1,T& p2)
{return p1.z<p2.z;}
cv::Point3f operator/(cv::Point3f& pp,float aa)
{return cv::Point3f(pp.x/aa,pp.y/aa,pp.z/aa);}
cv::Point2f operator/(cv::Point2f& pp,float aa)
{return cv::Point2f(pp.x/aa,pp.y/aa);}
void inputdata(string prx,string namepoint,vector<vector<cv::Point3f> >& inpoint)
{
	ifstream in((prx+namepoint).c_str());
	string line;
	while(getline(in,line))
	{
		stringstream sstr(line);
		int n=3;
		// sstr>>n;
		vector<cv::Point3f> point(3,cv::Point3f(-1,-1,-1));
		if(n==3)
		{
			sstr>>point[0].x>>point[0].y>>point[0].z>>point[1].x>>point[1].y>>point[1].z>>point[2].x>>point[2].y>>point[2].z;
			if(meetSortX(point,0.02))
				sort(point.begin(),point.end(),sortcoodx<cv::Point3f>);
			else if(meetSortY(point,0.02))
				sort(point.begin(),point.end(),sortcoody<cv::Point3f>);
			else
				sort(point.begin(),point.end(),sortcoodz<cv::Point3f>);
			if(cv::norm(point[0]-point[1])>cv::norm(point[1]-point[2]))
				swap(point[0],point[2]);
		}
		inpoint.push_back(point); 
	}
	in.close();in.clear();
}
void inputdata2D(string prx,string namepoint,vector<vector<cv::Point2f> >& inpoint2D)
{
	ifstream in((prx+namepoint).c_str());
	string line;
	while(getline(in,line))
	{
		stringstream sstr(line);
		int n=3;
	    sstr>>n;
		vector<cv::Point2f> point(3,cv::Point2f(-1,-1));
		if(n==3)
		{
			sstr>>point[0].x>>point[0].y>>point[1].x>>point[1].y>>point[2].x>>point[2].y;
			if(meetSortX(point,2))
				sort(point.begin(),point.end(),sortcoodx<cv::Point2f>);
			else if(meetSortY(point,2))
				sort(point.begin(),point.end(),sortcoody<cv::Point2f>);
			else
			{ 
				cout<<"some wrong! watch!!!!!"<<endl;
				break;
			}
			if(cv::norm(point[0]-point[1])>cv::norm(point[1]-point[2]))
				swap(point[0],point[2]);  
		}
		inpoint2D.push_back(point); 
	}
	in.close();in.clear();
}

void Calibration(vector<vector<cv::Point3f> >& inpoint,vector<vector<cv::Point2f> >& outimagpoint,const cv::Mat& camera,const cv::Mat& distC,const cv::Mat& rvec,const cv::Mat& tvec)
{
	assert(inpoint.size()==outimagpoint.size());
	vector<bool> indexi(inpoint.size(),true);
    for (int i=0;i<inpoint.size();i++)
	{
		for (int j=0;j<3;j++)
			if(inpoint[i][j].x==-1)
			    indexi[i]=false;
	}
	for (int i=0;i<outimagpoint.size();i++)
	{
		for (int j=0;j<3;j++)
			if(outimagpoint[i][j].x==-1)
				indexi[i]=false;
	}
// vector<Mat>  inpointCopy;
// vector<Mat>  outimagpointCopy;
	int countt=0;
	for (int i=0;i<outimagpoint.size();i++)
				if(indexi[i])countt++;
	double* obj=new double[countt*3*3];
	float* img=new float[countt*3*2];
	for (int i=0;i<inpoint.size();i++)
	{
		if (!indexi[i])
		{
			for(int j=0;j<3;j++)
            {
// 				inpointCopy.push_back(Mat(1,3,CV_32FC1,&inpoint[i][j].x));
// 		        outimagpointCopy.push_back(Mat(1,2,CV_32FC1,&outimagpoint[i][j].x));
				obj[i*9+j*3]=inpoint[i][j].x;obj[i*9+j*3+1]=inpoint[i][j].y;obj[i*9+j*3+2]=inpoint[i][j].z;
				img[i*6+j*2]=outimagpoint[i][j].x;img[i*6+j*2+1]=outimagpoint[i][j].y;
			}
		}
	}
	CvMat obgg=cvMat(1,countt,CV_64FC3,&obj[0]);
	CvMat imgg=cvMat(1,countt,CV_64FC2,&img[0]);
	CvMat* camerav=cvCreateMat(3,3,CV_64FC1);cvZero(camerav);
	CvMat* distCv=cvCreateMat(4,1,CV_64FC1);cvZero(distCv);
	CvMat* rvecv=cvCreateMat(3,3,CV_64FC1);cvZero(rvecv);
	CvMat* tvecv=cvCreateMat(1,3,CV_64FC1);cvZero(tvecv);
	vector<int> pp(1,obgg.cols);
	CvMat pait_c=cvMat(1,1,CV_32SC1,&pp[0]);
/*	cv::Mat distC;*/
//calibrateCamera(inpointCopyV,outimagpointCopyV,cvSize(640,240),camera,distC,rvec,tvec);
//(npoints->rows != 1 && npoints->cols != 1)

cvCalibrateCamera2(&obgg,&imgg,&pait_c,cvSize(640,240),camerav,distCv);
}
int _tmain(int argc, _TCHAR* argv[])
{
	//Mat obj();
  // calibrateCamera()
	vector<vector<cv::Point2f> > inpoint2D;
	vector<vector<cv::Point3f> > inpoint;
	inputdata(".\\","3D.txt",inpoint);
	inputdata2D(".\\","2d1.txt",inpoint2D);
	Mat cam(3,3,CV_32FC1);
	Mat distC(3,1,CV_32FC1);
	Mat revec(3,3,CV_32FC1);
	Mat tvec(3,1,CV_32FC1);
    Calibration(inpoint,inpoint2D,cam,distC,revec,tvec);
	CvMat mat = cam;
    cvSave("ca.xml",&mat);
	return 0;
}

